#ifndef _GAITPLANNER_H_
#define _GAITPLANNER_H_

#include "params.h"
#include "global.h"
#include <string>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

/**
 * @brief The GaitPlanner class represents a gait planner for a quadruped robot.
 *
 * This class is responsible for calculating the gait of the robot based on the
 * given parameters and current state.
 * It provides methods to set the gait parameters, set the robot's state, set
 * the desired motion, and calculate the gait.
 * The calculated gait can be accessed through various getter methods.
 */
class GaitPlanner
{
private:
    double t_; // 当前时间
    int n_;    // 当前迈步周期数

    // Gait scheduler
    int setflag_; // 调用函数前先设置步态
    double t_sw_; // 名义摆动相时间
    double t_st_; // 名义支撑相时间

    Vector4d Gd_;      // 占空比
    Vector4d Go_;      // 偏移量
    double T_gait_;    // = t_sw + t_st; //完整迈步周期,=t_sw_+t_st_
    double t_ng_;      // 当前周期的时间进度
    double t_ng_norm_; // = t_ng / T_gait; //归一化当前周期时间进度

    Vector4d t_fai_norm_; // 相位进度,i=0,1,2,3
    Vector4d s_fai_;      // 是否处于支撑相,i=0,1,2,3
    Vector4d t_st_norm_;  // 支撑相相位进度,i=0,1,2,3
    Vector4d t_sw_norm_;  // 摆动相相位进度,i=0,1,2,3

    // Swing phase planner
    Vector3d p_touch_com_O_;
    double psi_touch_;

    Vector3d p_t_com_O_;  // position of com in O, 来自状态估计器
    Vector3d v_t_com_O_;  // velocity of com in O, 来自状态估计器
    Matrix3d R_t_B_O_;    // rotation matrix from B to O, 来自状态估计器
    Vector3d omega_OB_O_; // angular velocity of the robot, 来自状态估计器
    double psi_t_;        // yaw angle of the robot, 来自状态估计器

    Vector3d v_t_com_d_B_; // desired velocity of com in B, 来自轨迹规划器
    double omega_t_z_d_;   // desired yaw rate of the robot, 来自轨迹规划器

    Vector3d getp_hip_i_B_(int i);
    Vector3d getp_touch_hip_i_O(int i); // 获取第i腿触地点在O系下的坐标,i=1,2,3,4

public:
    GaitPlanner();
    ~GaitPlanner();

    void setGait(double swingTime, double standTime, string mode); // 设置步态
    void calculateGait(double currentTime);                        // 计算步态

    void setState(Vector3d p_t_com_O, Vector3d v_t_com_O, Matrix3d R_t_B_O,
                  Vector3d omega_OB_O, double psi_t);          // 设置状态
    void setDesired(Vector3d v_t_com_d_B, double omega_t_z_d); // 设置期望

    Vector3d getp_swend_i_O(int i); // 获取第i腿摆动末端在O系下的坐标,i=1,2,3,4
    Vector4d gets_fai();            // 获取当前各腿支撑相状态
    // Vector3d getp_stend_i_O(int i); // 获取四足最近一次支撑相足底坐标
};

#endif